from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

def main():
    rclpy.init()
    nav=BasicNavigator()
    nav.waitUntilNav2Active()#等待导航激活可用
    # goal_pose=PoseStamped()
    # goal_pose.header.frame_id="map"
    # goal_pose.header.stamp=nav.get_clock().now().to_msg()
    while (1):
        num = int(input("请输入路店的数量num: "))
        count =1
        goal_poses = []
        while(count <= num):
            print(f"请输入第{count}个点的坐标")
            goal_pose=PoseStamped()
            goal_pose.header.frame_id="map"
            goal_pose.header.stamp=nav.get_clock().now().to_msg()
            goal_pose.pose.position.x=float(input("x: "))
            goal_pose.pose.position.y=float(input("y: "))
            goal_pose.pose.orientation.w=1.0#姿态（旋转），为1代表无旋转
            goal_poses.append(goal_pose)
            count +=1
        print(f"goal_poses = {goal_poses}")
        nav.followWaypoints(goal_poses)
        while not nav.isTaskComplete():
            feedback =nav.getFeedback()
            nav.get_logger().info(f'路点编号：{feedback.current_waypoint+1}')
            # nav.cancelTask()   # 超时取消
        result=nav.getResult()
        nav.get_logger().info(f'导航结果：{result}')
        
    # rclpy.spin(nav)
    # rclpy.shutdown()